#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/String.h> 
#include <std_msgs/Empty.h> 
#include <string>
#include <sstream>

using namespace std;
serial::Serial ser; //声明串口对象 
uint8_t serial_buff[200];
uint8_t recv_buff[200];
uint8_t send_buff[20];
size_t SerialRead_len=0;
std::string string_recv;
int lend_recv;
//回调函数 
// void write_callback(const std_msgs::String::ConstPtr& msg) 
// { 
//     ROS_INFO_STREAM("Writing to serial port" <<msg->data); 
//     ser.write(msg->data);   //发送串口数据 
// } 
int main (int argc, char** argv) 
{ 
    //初始化节点 
    ros::init(argc, argv, "serial_example_node"); 
    //声明节点句柄 
    ros::NodeHandle nh; 
    //订阅主题，并配置回调函数 
    // ros::Subscriber write_sub = nh.subscribe("write", 1000, write_callback); 
    //发布主题 
    // ros::Publisher read_pub = nh.advertise<std_msgs::String>("read", 1000); 
    try 
    { 
        //设置串口属性，并打开串口 
        ser.setPort("/dev/ttyUSB0"); 
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 

    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 
    //检测串口是否已经打开，并给出提示信息 
    if(ser.isOpen()) 
    { 
        ROS_INFO_STREAM("Serial Port initialized"); 
    } 
    else 
    { 
        return -1; 
    }
    //一些发送数据的测试
    string send_buf = "abcdefgh\n";
    int send_data = 0x12345;
    char  send_char[] = "111qwertrgsdfgsywrbstetbgdftetrthdedtrtyyuio222";
    struct send_MSG
    {
        int32_t a;
        int32_t b;
    };
    send_MSG send_msg = {123, 234};

    //指定循环的频率 
    ros::Rate loop_rate(20); 
    while(ros::ok()) 
    { 
        cout << endl;
        int i = 0;
        cout << "please input i:";
        cin >> i;
        if (i)
        {
            // 读取字符串数据（用于来自串口助手的字符发送）
            // string_recv=ser.read(i);
            // if (string_recv.length()) 
            // {
            //     cout << "lend_recv:" << string_recv.length() <<endl;
            //     for (int i =0;i<string_recv.length();i++)
            //     {
            //         printf("%c",string_recv.c_str()[i]);
            //     }
            //     cout << endl;
            // }
            // 读取uint8_t数据（用于地面站等结点的数据发送）
            lend_recv = ser.read(recv_buff,i); // 接收系统启动指令
            for (int i =0;i<lend_recv;i++)
            {
                printf("%#x",recv_buff[i]);
            }
            cout << endl;
        }
        
        // SerialRead_len=ser.read(serial_buff,ser.available());//
        // ros::Duration(0.05).sleep(); //sleep for 50 ms
        // ser.write(send_char,29);
        // if(ser.available())
        // {
        // ROS_INFO_STREAM("Reading from serial port\n"); 
        // std_msgs::String result; 
        // result.data = ser.read(ser.available()); 
        // ROS_INFO_STREAM("Read: " << result.data); 
        // // read_pub.publish(result); 
        // }
        // ser.write("send_char is ");   //发送串口数据
        // ser.write(send_char);   //发送串口数据

    //处理ROS的信息，比如订阅消息,并调用回调函数 
    ros::spinOnce(); 
    loop_rate.sleep(); 
    } 
}
